#include <ros/ros.h>
#include <ros/serialization.h>

#include "cg_interfaces/BridgeControl.h"
#include "cg_interfaces/BridgeSensor.h"
#include "cg_interfaces/ChassisStruct.h"

#include "cg_remote_control/udp_backend.hpp"

using namespace CG;

UDPBackEnd *udpBackend;
const int buffer_size = 1024;
const int hz = 50;
unsigned char serialize_buffer[buffer_size];
unsigned char rec_buffer[buffer_size];
std::string bridge_ip;
int id, self, bridge_port;
NetEndPoint target_point;
struct RosContext
{
    ros::Subscriber controlSub;
    ros::Publisher controlPub;
    ros::Subscriber sensorSub;
    ros::Publisher sensorPub;
    ros::Subscriber structSub;

    void Reset()
    {
        controlSub.shutdown();
        controlPub.shutdown();
        sensorSub.shutdown();
        sensorPub.shutdown();
    }
} context;

enum class MsgType : unsigned char
{
    Undefine = 0,
    BridgeControl = 1,
    BridgeSensor = 2,
    ChassisStruct = 3
};

enum class WorkMode
{
    Idel = 0,
    Master = 1,
    Slaver = 2
} mode;

void MasterControlCallback(const cg_interfaces::BridgeControl::ConstPtr &msg)
{
    // ROS_INFO("virtual_bridge: controlCallback called");
    serialize_buffer[0] = static_cast<unsigned char>(MsgType::BridgeControl);
    int length = ros::serialization::serializationLength(*msg);
    ros::serialization::OStream ostream(serialize_buffer + 1, length);
    ros::serialization::serialize(ostream, *msg);
    udpBackend->Send(serialize_buffer, length + 1, target_point);
}

void SlaverSensorCallback(const cg_interfaces::BridgeSensor::ConstPtr &msg)
{
    // ROS_INFO("virtual_bridge: sensorCallback called");
    serialize_buffer[0] = static_cast<unsigned char>(MsgType::BridgeSensor);
    int length = ros::serialization::serializationLength(*msg);
    ros::serialization::OStream ostream(serialize_buffer + 1, length);
    ros::serialization::serialize(ostream, *msg);
    udpBackend->Send(serialize_buffer, length + 1, target_point);
}

void ChangeROSContextAccordingMode(WorkMode mode);

void StructCallback(const cg_interfaces::ChassisStruct::ConstPtr &msg)
{
    std::string master_ip;
    ros::NodeHandle n;
    if (!n.getParam("/bridge/id" + std::to_string(msg->master) + "/ip", master_ip))
    {
        ROS_ERROR("virtual_bridge node: failed to load master ip");
        ros::shutdown();
        return;
    }
    if (msg->master == self)
    {
        if (self != id)
        {
            mode = WorkMode::Master;
            if (udpBackend != nullptr)
            {
                delete udpBackend;
                udpBackend = nullptr;
            }
            udpBackend = new UDPBackEnd(NetEndPoint(master_ip, bridge_port));
            target_point = NetEndPoint(bridge_ip, bridge_port);
        }
        else
        {
            mode = WorkMode::Idel;
            if (udpBackend != nullptr)
            {
                delete udpBackend;
                udpBackend = nullptr;
            }
        }
    }
    else
    {
        if (id == self)
        {
            mode = WorkMode::Slaver;
            if (udpBackend != nullptr)
            {
                delete udpBackend;
                udpBackend = nullptr;
            }
            udpBackend = new UDPBackEnd(NetEndPoint(bridge_ip, bridge_port));
            target_point = NetEndPoint(master_ip, bridge_port);
        }
        else
        {
            mode = WorkMode::Idel;
            if (udpBackend != nullptr)
            {
                delete udpBackend;
                udpBackend = nullptr;
            }
        }
    }
    ChangeROSContextAccordingMode(mode);
}

bool TryLoadParam(int &id, int &self, std::string &ip, int &port)
{
    ros::NodeHandle n("~");
    if (!n.getParam("id", id))
    {
        ROS_ERROR("virtual_bridge node: id not found");
        return false;
    }
    n.param("self", self, -1);
    ros::NodeHandle n_global;
    if (self == -1 && !n_global.getParam("self", self))
    {
        ROS_ERROR("virtual_bridge node: self not found");
        return false;
    }
    ros::NodeHandle localHandle("bridge/id" + std::to_string(id));
    if (!localHandle.getParam("port", port))
    {
        ROS_ERROR("virtual_bridge node: port not found");
        return false;
    }
    if (!localHandle.getParam("ip", ip))
    {
        ROS_ERROR("virtual_bridge node: ip not found");
        return false;
    }
    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "virtual_bridge");
    ROS_INFO("virtual_bridge node started");

    if (!TryLoadParam(id, self, bridge_ip, bridge_port))
    {
        ROS_ERROR("virtual_bridge node: failed to load parameters");
        ros::shutdown();
        return -1;
    }
    ROS_INFO("virtual_bridge node load params: id:%d, self:%d, ip:%s, port:%d", id, self, bridge_ip.c_str(),
             bridge_port);
    context.structSub = ros::NodeHandle().subscribe<cg_interfaces::ChassisStruct>("chassis_struct", 1, StructCallback);
    mode = WorkMode::Idel;
    ChangeROSContextAccordingMode(WorkMode::Idel);
    ros::spinOnce();

    ros::Rate rate(hz);
    ssize_t rec_length = 0;
    NetEndPoint remote_endpoint;
    while (ros::ok)
    {
        if (mode == WorkMode::Idel)
        {
        }
        else if (mode == WorkMode::Master)
        {
            // chassis -> bridge -> slaver :control
            // slaver -> bridge -> chassis :sensor
            while (udpBackend->Receive(rec_buffer, buffer_size, rec_length, remote_endpoint))
            {
                MsgType msg_type = static_cast<MsgType>(rec_buffer[0]);
                if (msg_type == MsgType::BridgeSensor)
                {
                    cg_interfaces::BridgeSensor msg;
                    ros::serialization::IStream istr(rec_buffer + 1, rec_length - 1);
                    ros::serialization::deserialize(istr, msg);
                    context.sensorPub.publish(msg);
                }
                else
                {
                    ROS_WARN("virtual_bridge node: unknown message type");
                }
            }
            ROS_INFO("empty receive buffer");
        }
        else
        {
            // chassis -> bridge -> master :sensor
            // master -> bridge -> chassis :control
            while (udpBackend->Receive(rec_buffer, buffer_size, rec_length, remote_endpoint))
            {
                MsgType msg_type = static_cast<MsgType>(rec_buffer[0]);
                if (msg_type == MsgType::BridgeControl)
                {
                    cg_interfaces::BridgeControl msg;
                    ros::serialization::IStream istr(rec_buffer + 1, rec_length - 1);
                    ros::serialization::deserialize(istr, msg);
                    context.controlPub.publish(msg);
                }
                // else if (msg_type == MsgType::ChassisStruct)
                //{
                //     cg_interfaces::ChassisStruct msg;
                //     ros::serialization::IStream istr(rec_buffer + 1, rec_length - 1);
                //     ros::serialization::deserialize(istr, msg);
                //     context.structPub.publish(msg);
                // }
                else
                {
                    ROS_WARN("virtual_bridge node: unknown message type");
                }
            }
            ROS_INFO("empty receive buffer");
        }
        ros::spinOnce();
        rate.sleep();
    }
    if (udpBackend != nullptr)
    {
        delete udpBackend;
    }
    ros::shutdown();
    return 0;
}

void ChangeROSContextAccordingMode(WorkMode mode)
{
    std::string topic_root = "bridge" + std::to_string(id) + '/';
    ros::NodeHandle nh;
    switch (mode)
    {
    case WorkMode::Idel:
        context.Reset();
        break;
    case WorkMode::Master:
        context.Reset();
        context.controlSub =
            nh.subscribe<cg_interfaces::BridgeControl>(topic_root + "control", 1, MasterControlCallback);
        context.sensorPub = nh.advertise<cg_interfaces::BridgeSensor>(topic_root + "sensor", 1);
        break;
    case WorkMode::Slaver:
        context.Reset();
        context.controlPub = nh.advertise<cg_interfaces::BridgeControl>(topic_root + "control", 1);
        context.sensorSub = nh.subscribe<cg_interfaces::BridgeSensor>(topic_root + "sensor", 1, SlaverSensorCallback);
        break;
    }
    ROS_INFO("virtual_bridge node: switch to %d mode", static_cast<int>(mode));
}